#include "../include/control_node.hpp"

void ControlNode::run(){
    auto hz = std::chrono::milliseconds(10);
    timer_ = this->create_wall_timer(
        hz, std::bind(&ControlNode::control, this));
}

ControlNode::ControlNode(const std::string &node_name) 
            : rclcpp::Node(node_name){
                publisher_ = this->create_publisher<std_msgs::msg::String>("control_publiser", 10);
                publisher_float_ = this->create_publisher<std_msgs::msg::Float32>("control_test", 10);
                control_ptr = std::make_shared<Control>();
            }

void ControlNode::control(){
    ControlOutput control_out = control_ptr->CalculateControl();
    RCLCPP_INFO(get_logger(),"角度:%f 油门:%f 刹车:%f", control_out.steer_angle, control_out.throttle, control_out.brake_value);
}

int main(int argc, char** argv){
    rclcpp::init(argc, argv);
    auto ptr = std::make_shared<ControlNode>("control_node");
    ptr->run();
    rclcpp::spin(ptr);
    return 0;
}

